#include <opencv2/highgui.hpp>
#include <iostream>
#include <algorithm>

#if 0
#include "ren/Pipeline.h"
#include "ren/FPSCamera.h"
#include "ren/Mesh.h"
#include "ren/Transform3D.h"
#include "ren/ObjModel.h"
using namespace SoftRenderer;

void pipeline() {
	int width = 640, height = 512;
	
	Pipeline pipeline(width, height);
	pipeline.initialize();
  pipeline.setShadingMode(ShadingMode::Simple);

	FPSCamera fpsCamera(Vector3D(-0.0f, 5.0f, 7.0f));
	fpsCamera.rotate(FPSCamera::LocalRight, -30.0f);

	pipeline.setViewMatrix(fpsCamera.getPosition(), fpsCamera.getViewMatrix());
	pipeline.setProjectMatrix(45.0f, static_cast<float>(width)/height,0.01f, 40.0f);
	pipeline.setPolygonMode(PolygonMode::Fill);
	pipeline.setGeometryCliping(true);
	pipeline.setBackFaceCulling(true);
	pipeline.setDepthTesting(true);

#if 0
	Mesh cube;
	cube.asBox(1.0, 1.0, 1.0);
	Transform3D cubeTransform;
	cubeTransform.setTranslation(Vector3D(3.0f, -1.0f, +2.0f));
	pipeline.setVertexBuffer(&cube.m_vertices);
	pipeline.setIndexBuffer(&cube.m_indices);
#endif

#if 1
	ObjModel obj("data/rbot/model/ape.obj");
	Transform3D pose;
	pose.setTranslation(Vector3D(3.0f, -1.0f, +2.0f));
	pipeline.setVertexBuffer(&obj.m_vertices);
	pipeline.setIndexBuffer(&obj.m_indices);
#endif

	pipeline.clearFrameBuffer(Vector4D(0.0,0.0,0.0,1.0f));
	pipeline.drawObjectMesh();

	cv::Mat buf(512, 640, CV_8UC4, pipeline.getFrameResult());
	cv::imshow("ren", buf);
	cv::waitKey();
}
#endif

#include "mbt/model.hh"
#include "geometry.h"
#include "mbt/object3d.hh"
#include "base/pose_io.hh"

const double tolerance = 1e-5f;
inline bool equal(float a, float b) {
    return fabs(a - b) < tolerance;
}

class FrameBuffer {
public:
	FrameBuffer(Mat44f& K, int w, int h) {
		kmat = K;
		width = w, height = h;
		
		mask_buf.resize(width * height, 0);
		depth_buf.resize(width * height, 1.0f);

		mask_data = mask_buf.data();
		depth_data = depth_buf.data();
	}

	void DrawModel(Model* model) {
		Project(model);

		std::vector<unsigned int>& idxs = model->indices;

		for (int i = 0; i < idxs.size(); i += 3) {
			const Vec2f& p0 = ptHs[idxs[i]];
			const Vec2f& p1 = ptHs[idxs[i+1]];
			const Vec2f& p2 = ptHs[idxs[i+2]];

			DrawFaceFilled(p0, p1, p2, 255);
		}
	}

	void Project(Model* model) {
		cv::Matx44f cvm = model->getPose();
		Mat44f mv;
		for (int r = 0; r < 4; ++r)
		for (int c = 0; c < 4; ++c) {
			mv[r][c] = cvm(r, c);
		}

		ptHs.resize(model->vertices.size());
		for (int i = 0; i < model->vertices.size(); ++i) {
			Vec4f pt;
			pt[0] = model->vertices[i](0);
			pt[1] = model->vertices[i](1);
			pt[2] = model->vertices[i](2);
			pt[3] = 1.0f;
			Vec4f ptH = kmat * mv * pt;

			ptHs[i][0] = ptH[0]/ptH[2];
			ptHs[i][1] = ptH[1]/ptH[2];
		}

		//idxs = model->indices;
	}

	void DrawFaceFilled(const Vec2f& pt0, const Vec2f& pt1, const Vec2f& pt2, uchar v) {
		Vec2f tmp;
		Vec2f target[3] = {pt0, pt1, pt2};
		if (target[0].y > target[1].y) {
			tmp = target[0];
			target[0] = target[1];
			target[1] = tmp;
		}
		if (target[0].y > target[2].y) {
			tmp = target[0];
			target[0] = target[2];
			target[2] = tmp;
		}
		if (target[1].y > target[2].y) {
			tmp = target[1];
			target[1] = target[2];
			target[2] = tmp;
		}

    if (equal(target[1].y, target[2].y)) {
			DrawBottomFace(target[0], target[1], target[2], v);
    } else if (equal(target[0].y, target[1].y)) {
			DrawTopFace(target[0], target[1], target[2], v);
    } else {
			float weight = float(target[1].y-target[0].y)/(target[2].y-target[0].y);
			Vec2f new_point(lerp(target[0].x, target[2].x, weight), target[1].y);
			DrawBottomFace(target[0], new_point, target[1], v);
			DrawTopFace(target[2],target[1],new_point, v);
    }
	}

	uchar* mask_data;
	float* depth_data;

private:
	void RastLine(int sx, int ex, int y, uchar v) {
		sx = y * width + sx;
		ex = y * width + ex;
		for (int i = sx; i <= ex+1; ++i) {
			mask_data[i] = v;
		}
	}

	void DrawPixel(const uint& x, const uint& y, uchar v) {
		uint idx = y * width + x;
		mask_data[idx] = v;
	}

	void RastLine(float sx, float ex, float y, uchar v) {
		int len = ex - sx + 1;
		for (int i = 0; i <= len; ++i) {
			DrawPixel(sx + i, y, v);
		}
	}

	void RastLine(const Vec2f& sv, const Vec2f& ev, uchar v) {
		int len = ev.x - sv.x + 1;
		for (int i = 0; i <= len; ++i) {
			DrawPixel(sv.x + i, sv.y, v);
		}
	}

	float lerp(float s, float e, float w) {
		return s + (e - s) * w;
	}

	Vec2f lerp(const Vec2f& sv, const Vec2f& ev, float w) {
		return Vec2f(sv + (ev - sv) * w);
	}

	void DrawTopFace(const Vec2f& bpt, const Vec2f& lpt, const Vec2f& rpt, uchar v) {
		const int hy = bpt.y - lpt.y + 1;

		if (lpt.x < rpt.x) {
			for (int i = 0; i <= hy; ++i) {
				float weight = float(i) / hy;
				//RastLine(lerp(lpt, bpt, weight), lerp(rpt, bpt, weight), v);
				RastLine(lerp(lpt.x, bpt.x, weight), lerp(rpt.x, bpt.x, weight), lerp(lpt.y, bpt.y, weight), v);
			}
		}	else {
			for (int i = 0, y = lpt.y; i <= hy; ++i, ++y) {
				float weight = float(i) / hy;
				//RastLine(lerp(rpt, bpt, weight), lerp(lpt, bpt, weight), v);
				RastLine(lerp(rpt.x, bpt.x, weight), lerp(lpt.x, bpt.x, weight), lerp(lpt.y, bpt.y, weight), v);
			}
		}
	}

	void DrawBottomFace(const Vec2f& tpt, const Vec2f& lpt, const Vec2f& rpt, uchar v) {
		const int hy = lpt.y - tpt.y + 1;

		if (lpt.x < rpt.x) {
			for (int i = 0; i < hy; ++i) {
				float weight = float(i) / hy;
				//RastLine(lerp(tpt.x, lpt.x, weight), lerp(tpt.x, rpt.x, weight), lerp(tpt.y, lpt.y, weight), v);
				RastLine(lerp(tpt.x, lpt.x, weight), lerp(tpt.x, rpt.x, weight), lerp(tpt.y, lpt.y, weight), v);
			}
		}	else {
			for (int i = 0, y = tpt.y; i < hy; ++i, ++y) {
				float weight = float(i) / hy;
				//RastLine(lerp(tpt, rpt, weight), lerp(tpt, lpt, weight), v);
				RastLine(lerp(tpt.x, rpt.x, weight), lerp(tpt.x, lpt.x, weight), lerp(tpt.y, lpt.y, weight), v);
			}
		}
	}

	Mat44f kmat;

	std::vector<Vec2f> ptHs;
	std::vector<unsigned int> idxs;

	std::vector<uchar> mask_buf;
	std::vector<float> depth_buf;
	int width, height;
};

void test() {
	Mat44f kmat;
	kmat = Mat44f::identity();
	kmat[0][0] = 650.048f;
	kmat[1][1] = 647.183f;
	kmat[0][2] = 324.328f;
	kmat[1][2] = 257.323f;

	//kmat[0][0] = 650.048f/2;
	//kmat[1][1] = 647.183f/2;
	//kmat[0][2] = 324.328f/2;
	//kmat[1][2] = 257.323f/2;

	//kmat[0][0] = 650.048f/4;
	//kmat[1][1] = 647.183f/4;
	//kmat[0][2] = 324.328f/4;
	//kmat[1][2] = 257.323f/4;

	std::vector<int> fids;
	std::vector<std::vector<cv::Matx44f> > tk_poses;
	PoseReader pose_reader;
	std::string config_file;
	pose_reader.Read("data/rbot/gt_pose.txt", config_file, fids, tk_poses);

	std::vector<float> distances = { 200.0f, 400.0f, 600.0f };
	Object3D* obj = new Object3D("data/rbot/smodel/lamp.obj", cv::Matx44f(), 1.0, 0.55f, distances);
	obj->setPose(tk_poses[0][0]);

	int width = 640, height = 512;
	//int width = 320, height = 256;
	//int width = 160, height = 128;
	FrameBuffer buf(kmat, width, height);

	int64 time0 = cv::getTickCount();
	for (int i = 0; i < 1000; ++i)
	buf.DrawModel(obj);
	int64 time1 = cv::getTickCount();
	std::cout << (time1-time0) / cv::getTickFrequency() << std::endl;
	//buf.DrawFaceFilled(Vec2f(0, 0), Vec2f(100, 100), Vec2f(0, 40), 255);
	cv::Mat obuf(height, width, CV_8UC1, buf.mask_data);
	cv::imshow("ren", obuf);
	cv::waitKey();
}

void main() {
	test();
}